#include<ros/ros.h>
#include<std_msgs/Bool.h>

#include <tf/transform_listener.h>

#include<sensor_msgs/LaserScan.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include <laser_geometry/laser_geometry.h>

#include <std_msgs/String.h>

#include <actionlib/server/simple_action_server.h>
#include <approach_grasp_point/approach_grasp_pointAction.h>

#include <nav_msgs/Odometry.h>

#include <pcl/common/common.h>

#include <pcl_conversions/pcl_conversions.h>

typedef actionlib::SimpleActionServer<approach_grasp_point::approach_grasp_pointAction> approach_action_server;
typedef approach_grasp_point::approach_grasp_pointFeedback approach_feedback_;
typedef approach_grasp_point::approach_grasp_pointResult approach_result_;

class motionPlanner
{
public:
    motionPlanner(int argc,char* argv[]):nh("~"),
        listener_(ros::Duration(1000)),
        distance_to_graspPoint(1000),
        laserDataReceived(false)
    {
        approach_as = new approach_action_server(nh, "/approach_graspPoint",boost::bind(&motionPlanner::executeCB, this, _1), false);
        point_cloud_pub = nh.advertise<sensor_msgs::PointCloud>("/cloud_out",1000);
        cmd_vel_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel",1000);


        nh.param<double>( "x_filter_min", x_filter_min, 0.30 );
        nh.param<double>( "z_filter_max", z_filter_max, 1.00 );
        nh.param<double>( "z_filter_min", z_filter_min, 0.20 );
        nh.param<std::string>( "topic", scan_topic, "/urg_scan");
        nh.param<std::string>("laser_frame", laser_frame, "/urg_laser");
    }
    int run();

private:
    void laser_sub(const sensor_msgs::LaserScanConstPtr&);
    void odom_sub(const nav_msgs::OdometryConstPtr&);
    void executeCB(const approach_grasp_point::approach_grasp_pointGoalConstPtr &goal);
private:
    approach_action_server *approach_as;
    approach_feedback_ approach_feedback;
    approach_result_ approach_result;
    ros::Subscriber laser_scan_sub;
    ros::Subscriber odometery_sub;
    ros::Publisher point_cloud_pub;
    ros::Publisher cmd_vel_pub;
    ros::NodeHandle nh;
    laser_geometry::LaserProjection projector_;
    tf::TransformListener listener_;
    std::string scan_topic;
    std::string laser_frame;
    double x_filter_min;
    double z_filter_max;
    double z_filter_min;
    double distance_to_graspPoint;
    bool laserDataReceived;
    nav_msgs::Odometry currentOdom;
};
/*##########################################################*/
int motionPlanner::run()
{
    ros::Rate loop_rate(10);
    odometery_sub = nh.subscribe<nav_msgs::Odometry>("/odom",1000,
                                                     (boost::function<void(const nav_msgs::OdometryConstPtr&)>)boost::bind(&motionPlanner::odom_sub,this,_1));
    laser_scan_sub=nh.subscribe<sensor_msgs::LaserScan>(scan_topic,1000,
                                                        (boost::function < void(const sensor_msgs::LaserScanConstPtr&)>) boost::bind( &motionPlanner::laser_sub, this, _1 ));
    /*while(ros::ok() && !laserDataReceived)
    {
        ROS_ERROR("Laser data does not received, retrying....");
        ros::spinOnce();
        loop_rate.sleep();
    }
    ROS_INFO("Laser data received, statring action server");
*/
    approach_as->start();

    while(ros::ok())
    {
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}
/*##########################################################*/
void motionPlanner::laser_sub(const sensor_msgs::LaserScanConstPtr &msg)
{
    laserDataReceived = true;

    sensor_msgs::PointCloud cloud;
    sensor_msgs::PointCloud filtered;
    sensor_msgs::PointCloud roi;
    sensor_msgs::PointCloud2 filtered2;

    try
    {
        if( listener_.waitForTransform(laser_frame,"/base_link",ros::Time(0),ros::Duration(50) ) )
        {
            projector_.transformLaserScanToPointCloud("base_link",*msg,
                                                      cloud,listener_);
            for(unsigned int i=0;i<cloud.points.size();i++)
            {
                if( cloud.points[i].z < z_filter_max &&
                    cloud.points[i].z > z_filter_min &&
                    cloud.points[i].x > x_filter_min )
                {
                    filtered.points.push_back(cloud.points[i]);
                }
            }

            filtered.header.frame_id = "/base_link";
            filtered.header.stamp = ros::Time::now();
            roi.header.frame_id = "/base_link";
            roi.header.stamp = ros::Time::now();

            sensor_msgs::convertPointCloudToPointCloud2(filtered,filtered2);
            pcl::PointCloud<pcl::PointXYZ> pcl_filtered;
            pcl::fromROSMsg(filtered2,pcl_filtered);
            pcl::PointXYZ min;
            pcl::PointXYZ max;
            pcl::getMinMax3D(pcl_filtered,min,max);
            //std::cout<<min.x<<"   "<<max.x<<std::endl;
            //distance_to_graspPoint = min.x;
            distance_to_graspPoint = 0;

            for(unsigned int i=0;i<filtered.points.size();i++)
            {
                if(fabs( filtered.points[i].x - min.x) < 0.02 )
                {
                    roi.points.push_back(filtered.points[i]);
                    distance_to_graspPoint+= filtered.points[i].x;
                }
            }
            distance_to_graspPoint /= roi.points.size();
            distance_to_graspPoint *= 100;
            std::cout<<"DISTANCE "<<distance_to_graspPoint<<std::endl;
            //distance_to_graspPoint = floor(distance_to_graspPoint);
            point_cloud_pub.publish(roi);
        }
    }
    catch(tf::ExtrapolationException e)
    {

    }
}
void motionPlanner::odom_sub(const nav_msgs::OdometryConstPtr &msg)
{
    currentOdom = *msg;
}
/*##########################################################*/
void motionPlanner::executeCB(const approach_grasp_point::approach_grasp_pointGoalConstPtr &goal)
{
    geometry_msgs::Twist cmd_vel;
    ROS_INFO("Action Request received");
    if( distance_to_graspPoint > 75 || distance_to_graspPoint == NAN)
    {
        ROS_ERROR("Distance to grasp point is too far or NAN, Aborting action...");
            approach_as->setAborted();
            return;
    }

    while( distance_to_graspPoint > goal->distance * 100)
    {
        std::cout<<distance_to_graspPoint<<"  "<<goal->distance*100<<std::endl;
        cmd_vel.linear.x = 0.1;
        cmd_vel.angular.z = 0;
        approach_feedback.current_distance = distance_to_graspPoint;
        approach_as->publishFeedback(approach_feedback);
        cmd_vel_pub.publish(cmd_vel);
        ros::spinOnce();
    }
    while( ( (goal->distance * 100)- distance_to_graspPoint  ) > 0.001 )
    {
        ROS_INFO("Moving backward");
        cmd_vel.linear.x = -0.1;
        cmd_vel.angular.z = 0;
        approach_feedback.current_distance = distance_to_graspPoint;
        approach_as->publishFeedback(approach_feedback);
        cmd_vel_pub.publish(cmd_vel);
        ros::spinOnce();
    }


        cmd_vel.linear.x = 0;
        cmd_vel.angular.z = 0;
        cmd_vel_pub.publish(cmd_vel);
        approach_result.final_distance = distance_to_graspPoint;
        approach_as->setSucceeded(approach_result);
        ROS_INFO("ACTION Succeeded");
    /*
    geometry_msgs::Twist cmd_vel;
    nav_msgs::Odometry firstOdom = currentOdom;
    nav_msgs::Odometry minOdom = firstOdom;

    minOdom.pose.pose.orientation.z = 1000;
    float min = 1000;
    float firstDist = distance_to_graspPoint;
    ros::Rate loop(10);
    bool turnToLeft = false;

    while( fabs(currentOdom.pose.pose.orientation.z - firstOdom.pose.pose.orientation.z ) < 0.25 )
    {
        cmd_vel.angular.z = -0.07;
        cmd_vel.linear.x = 0;
        cmd_vel_pub.publish(cmd_vel);
        if( (distance_to_graspPoint - firstDist) > 1.00 )
        {
            ROS_INFO("Turn to Left true");
            turnToLeft = true;
            break;
        }
        loop.sleep();
        ros::spinOnce();
    }

    if(turnToLeft)
    {
        float result = 1000;
        do
        {
            ROS_INFO("Turn To Left");
            cmd_vel.angular.z = 0.07;
            cmd_vel.linear.x = 0;
            cmd_vel_pub.publish(cmd_vel);
            if((distance_to_graspPoint - min) < -2.50 )
                ROS_INFO("Decreasing");
            else
            if((distance_to_graspPoint - min) > -2.50)
                ROS_INFO("Increasing");
            else
                ROS_INFO("equal");
            if(distance_to_graspPoint<min)
            {
                min = distance_to_graspPoint;
                minOdom = currentOdom;
            }
            result = min-distance_to_graspPoint;
            loop.sleep();
            ros::spinOnce();
        }while( result > -2.50 );

        while(fabs( currentOdom.pose.pose.orientation.z - minOdom.pose.pose.orientation.z ) > 0.005)
        {
            ROS_INFO("Recovering to right");
            cmd_vel.angular.z = -0.07;
            cmd_vel.linear.x = 0;
            cmd_vel_pub.publish(cmd_vel);
            loop.sleep();
            ros::spinOnce();
        }
    }
    else
    {
        float result = 1000;
        do
        {
            ROS_INFO("Turn To Right");
            cmd_vel.angular.z = -0.07;
            cmd_vel.linear.x = 0;
            cmd_vel_pub.publish(cmd_vel);
            if((distance_to_graspPoint - min) < -2.50 )
                ROS_INFO("Decreasing");
            else
                if((distance_to_graspPoint - min) > -2.50)
                    ROS_INFO("Increasing");
                else
                    ROS_INFO("equal");
            if(distance_to_graspPoint<min)
            {
                min = distance_to_graspPoint;
                minOdom = currentOdom;
            }
            result = min-distance_to_graspPoint;
            loop.sleep();
            ros::spinOnce();
        }while( result > -2.50 );

        while(fabs( currentOdom.pose.pose.orientation.z - minOdom.pose.pose.orientation.z ) > 0.005)
        {
            ROS_INFO("Recovering to left");
            cmd_vel.angular.z = 0.07;
            cmd_vel.linear.x = 0;
            cmd_vel_pub.publish(cmd_vel);
            loop.sleep();
            ros::spinOnce();
        }
    }
    ROS_INFO("Stopping");
    cmd_vel.linear.x = 0;
    cmd_vel.angular.z = 0;
    cmd_vel_pub.publish(cmd_vel);
    approach_as->setSucceeded(approach_result);*/
}
/*##########################################################*/
int main(int argc,char* argv[])
{
    ros::init(argc,argv,"approch_grasp_point");

    motionPlanner fds(argc,argv);
    return fds.run();
}
